#include "IDW.h"

#define IMAGE_WIDTH 32
#define IMAGE_HEIGHT 24

#define P0 (0.077847)
#define P1 (0.123317 + 0.077847)
#define P2 (0.195346 + 0.123317 + 0.123317 + 0.077847)

int ktype = 0;
const int8_t offset[4][9] =
    {
        {-33, -32, -32,
         -1, 0, 0,
         -1, 0, 0},
        {-32, -32, -31,
         0, 0, 1,
         0, 0, 1},
        {-1, 0, 0,
         -1, 0, 0,
         31, 32, 32},
        {0, 0, 1,
         0, 0, 1,
         32, 32, 33},
};

// const int8_t offset[4][9] =
//     {
//         {-65, -64, -64,
//          -2, 0, 0,
//          -2, 0, 0},
//         {-64, -64, -63,
//          0, 0, 2,
//          0, 0, 2},
//         {-2, 0, 0,
//          -2, 0, 0,
//          63, 64, 64},
//         {0, 0, 2,
//          0, 0, 2,
//          64, 64, 65},
// };

const float kernel[3][9] =
    {
        //Signa=0.5
        {0.024879, 0.107973, 0.024879,
         0.107973, 0.468592, 0.107973,
         0.024879, 0.107973, 0.024879},
        //Sigma = 1.0
        {0.077847, 0.123317, 0.077847,
         0.123317, 0.195346, 0.123317,
         0.077847, 0.123317, 0.077847},
        //Sigma=2.0
        {0.102059, 0.115349, 0.102059,
         0.115349, 0.130371, 0.115349,
         0.102059, 0.115349, 0.102059}};

//Q15
const uint16_t kernelInt[3][9] =
    {
        //Signa=0.5
        {
            815,
            3538,
            815,
            3538,
            15355,
            3538,
            815,
            3538,
            815,
        },
        {
            2551,
            4041,
            2551,
            4041,
            6401,
            4041,
            2551,
            4041,
            2551,
        },
        {
            3344,
            3780,
            3344,
            3780,
            4272,
            3780,
            3344,
            3780,
            3344,
        }};

/**
 * @brief 高斯插值 2倍
 * 
 * @param src 输入
 * @param w 输入宽
 * @param h 输入高
 * @param dest 
 */
void idwGauss(int16_t *src, uint16_t w, uint16_t h, float *dest)
{
    float pix;
    //For rest of  output pixel:
    for (int i = 0; i < w * h * 4; i++)
    {
        int sourceAddress = ((i >> 1) & 0x1f) + ((i & 0xffffff80) >> 2);
        pix = 0;
        uint16_t q = (i & 1) + ((i & 0x40) >> 5); //Calculation to perform
        for (int z = 0; z < 9; z++)
        {
            int sa = sourceAddress + offset[q][z];
            if (sa > 0 && sa < w * h)
                pix += kernel[ktype][z] * src[sa];
            dest[i] = pix;
        }
    }
}

/**
 * @brief 高斯插值 2倍 定点运算
 * 
 * @param src 输入
 * @param w 输入宽
 * @param h 输入高
 * @param dest 
 */
void idwGaussInt(int16_t *src, uint16_t w, uint16_t h, int16_t *dest)
{
    float pix;
    //For rest of  output pixel:
    for (int i = 0; i < w * h * 4; i++)
    {
        int sourceAddress = ((i >> 1) & 0x1f) + ((i & 0xffffff80) >> 2);
        pix = 0;
        uint16_t q = (i & 1) + ((i & 0x40) >> 5); //Calculation to perform
        for (uint16_t z = 0; z < 9; z++)
        {
            int sa = sourceAddress + offset[q][z];
            if (sa > 0 && sa < w * h)
                pix += (kernelInt[ktype][z] * (int32_t)(src[sa])) >> 15;
            dest[i] = pix;
        }
    }
}
